//
// Created by lqk on 2022/2/5.
//

#include "HighFrequencyTasksWrapper.h"

void HighFrequencyTasksWrapper::Init() {
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
}

void HighFrequencyTasksWrapper::CleanUp() {

}

void HighFrequencyTasksWrapper::RunOnce() {
    m_stateEstimator.Update();
    m_legsController.UpdateLegsState(m_legState);
    m_convexMPC.Update(&m_estimatedState,m_legCommand,m_legState,&m_quadKinematics);
//    m_legCommand[0].m_footForce(2)=-50;
//    for(int i=0;i<4;i++){
//        m_legCommand[i].m_footPosDesired<<0,0,-0.3;
//        m_legCommand[i].m_kpCartesian<<700,0,0,0,700,0,0,0,200;
//        m_legCommand[i].m_kdCartesian<<10,0,0,0,10,0,0,0,10;
//    }
    m_legsController.UpdateMotorsCommand(m_legCommand,m_legState);

}

void HighFrequencyTasksWrapper::PrintStatus() {
//    std::cout<<m_name<<" is running!"<<std::endl;
//    for(int leg=0;leg<4;leg++){
////        std::cout<<m_legState[leg].m_q<<std::endl;
////        std::cout<<m_legState[leg].m_qd<<std::endl;
////        std::cout<<m_legState[leg].m_jacobian<<std::endl;
////        std::cout<<m_legState[leg].m_footVel.transpose()<<std::endl;
////        std::cout<<m_legState[leg].m_footPos.transpose()<<std::endl;
//    }

}